//库
#include <Adafruit_NeoPixel.h>    //灯珠控制
#include <Servo.h>                //舵机
#include "SoftwareSerial.h"       //串口通讯
#include "I2Cdev.h"               //I2C通讯
#include "MPU6050.h"              //陀螺仪
#include "Wire.h"                 //I2C通讯库
#include <math.h>                 //计算库

//全局变量及常量
#define led_Numbers 131                   //灯带上灯珠个数
#define SCL D1                            //I2C时钟->MPU6050 
#define SDA D2                            //I2C数据<-MPU6050
#define button_1 D3                       //主按钮
#define crystal_LED D4                    //水晶灯光
#define cabin_Servo D5                    //水晶舱门舵机
#define muzzle_LED D6                     //剑口灯光
#define button_LED D0                     //主按钮灯光
#define saber_LED D7                      //剑刃灯光

unsigned int state=0;                     //记录剑刃状态
unsigned int R=0,G=0,B=255;               //剑刃RGB颜色,蓝色
unsigned int button_state=0;              //记录主按钮状态
bool saber_awaken= false ;        //记录状态是休眠还是唤醒
unsigned int count=0;                     //计数变量
int16_t ax, ay, az;                       //三个方向上的角速度
int16_t gx, gy, gz;                       //三个方向上的加速度
long long int sq_a,sq_g,result_a,result_g;//记录陀螺仪数据
int vol=15;                               //初始化音量

//创建对象
Adafruit_NeoPixel saber = Adafruit_NeoPixel(led_Numbers, saber_LED, NEO_GRB + NEO_KHZ800);  //NEO_KHZ400+NEO_RGB 灯带控制
SoftwareSerial LS_Serial(D9, D10);      //mini_mp3_player 异步串行通讯
MPU6050 mpu; //MPU6050 I2C通讯

//初始化设置
void setup(){
  Wire.begin();                           //可修改默认I2C通讯端口，默认SCL为D1，SDA为D2，其中SCL可改为引脚10，但修改后因未知原因陀螺仪未起效
  Serial.begin(9600);                     //串口波特率设置，可能是与电脑进行串口通讯用的，或许可以删掉试试
  LS_Serial.begin(9600);                  //mp3串口波特率设置
  Serial.println("Initializing I2C devices...");//这几行println不知道为啥删了就不好使
  mpu.initialize();                       //陀螺仪初始化
  Serial.println("Testing device connections...");
  Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");//陀螺仪检测连接
  
  attachInterrupt(digitalPinToInterrupt(button_1), InterruptFunc_1, FALLING);               //主按钮中断允许，下降沿触发
  saber.begin();                          //灯带控制初始化
  saber.setBrightness(180);               //灯带亮度，最高好像是180
  pinMode(button_LED, OUTPUT);            //设置主按钮灯光为输出
  pinMode(button_1, INPUT_PULLUP);        //设置主按钮为输入，并给它接一个上拉电阻使其自复位
  program_Started();                      //程序开始
}
//主循环
void loop(){
  //if(!saber_awaken){
    
  //}
  if((state==1)&&(button_state==1)){
    button_state=0;
    SaberOPEN();
    SmoothSwing();
  }
  else if((state==0)&&(button_state==1)){
    button_state=0;
    SaberCLOSE();
  }
   
}
//函数
void program_Started(){                     //程序开始
  mp3_Commander(0x3F, 0, 0);
  setVolume(15);
  digitalWrite(button_LED, LOW); 
}
void setVolume(int volume)
{
  mp3_Commander(0x06, 0, volume); // Set the volume (0x00~0x30)
}

ICACHE_RAM_ATTR void InterruptFunc_1(){      //主按钮中断函数
  long long int count=0;
  for(int j=0;j<255;j++){
    for(int k=0;k<255;k++){
      for(int t=0;t<255;t++){
        t=t*1;
      }
    }
  }
  delayMicroseconds(10000);
  if(state==0){
    state=1;
    button_state=1;
  }
  else{
    state=0;
    button_state=1;
  }
  while((digitalRead(button_1)==0)){
    count++;
    if(count>=10000000){
      count=3000000;
    }
  }
  if(count>=3000000){
  }
}

void mp3_Commander(byte CMD, byte Par1, byte Par2){               //编写串口通讯协议，发出控制指令
  word checksum = -(0xFF + 0x06 + CMD + 0x00 + Par1 + Par2);   
  byte Command_line[10] = { 0x7E, 0xFF, 0x06, CMD, 0x00 ,Par1, Par2, highByte(checksum), lowByte(checksum), 0xEF};                          
  for (byte i=0; i<10; i++){
    LS_Serial.write( Command_line[i]);
  }
  delay(10);
}

void SaberOPEN(){                 //剑刃开 音效 及光效
  int j;
  setVolume(30);
  mp3_Commander(0x03,0x00,0x01);
  digitalWrite(button_LED, HIGH);  
  for(int i=count;i<led_Numbers;i+=1){
    if(state!=1){
      break;
    }
    R+=2;
    if(R>250){
      R-=2;
    }
    saber.setPixelColor(i,R,G,B);  
    saber.show();
    count=i;
    j=100/(i+10);
    delay(j);
  }
  R=0;
  delay(200);
  mp3_Commander(0x0E,0x00,0x00);
  setVolume(15);
}
void SaberCLOSE(){                //剑刃关 音效 及光效
  int j;
  mp3_Commander(0x0E,0x00,0x00);
  setVolume(30);
  mp3_Commander(0x03,0x00,0x02);
  mp3_Commander(0x0D,0x00,0x02);
  delay(10);
  for(int i=count;i>=0;i-=1){
    if(state!=0){
      break;
    }
    saber.setPixelColor(i,0,0,0);   
    saber.show();
    count=i;
    j=100/(i+10);
    delay(j);
  }
  digitalWrite(button_LED, LOW);  
}
void SmoothSwing(){                 //嗡嗡嗡函数
  mp3_Commander(0x03,0x00,0x03); 
  mp3_Commander(0x0D,0x00,0x03);
  while(1){
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);   //获取mpu6050数据，分别是三轴加速度和三轴角速度
    sq_a=pow(ax,2)+pow(ay,2)+pow(az,2);             //MAX=3*32768^2
    result_a=abs(sqrt(sq_a)-16130);   //重力加速度矫正-16130，MAX=3^0.5*32768-16130=40625
    sq_g=pow(gx,2)+pow(result_a,2)+pow(gz,2);             //MAX=2*32768^2+40625^2
    result_g=abs(sqrt(sq_g)-500);     //地球自转角速度矫正-500,MAX=61126 
    vol=15+(result_g/4076);           //根据角速度线性调整音量，4076=61126/15
    setVolume(vol);                   //设置音量 
    if(state!=1){
      break;
    }
  }
}
